/*******************************************************************************
 * Copyright (c) SCUT. 2022. All rights reserved.
 * Description: The realization of SYTRS_3.
 * Author: CHEN Han
 * Create: 2022-06-26
 *******************************************************************************/

#include "SYTRS_3.h"

void SYTRS_3(const char* uplo,
             const int* n,
             const int* nrhs,
             dataType* A,
             const int* lda,
             dataType* E,
             int* ipiv,
             dataType* B,
             const int* ldb,
             int* info)
             {
#ifdef COMPLEX
    const dataType CONE = 1;
    const dataType NEG_CONE = -1;
#endif
#ifdef COMPLEX16
    const dataType CONE = 1;
    const dataType NEG_CONE = -1;
#endif

    const int N = *n;
    const int NRHS = *nrhs;
    const int LDA = *lda;
    const int LDB = *ldb;
    int upper;
    int i, j, k, kp;
    dataType ak, akm1, akm1k, bk, bkm1, deNom;

    *info = 0;
    upper = KmlLsame(*uplo, 'U');
    if (!upper && !KmlLsame(*uplo, 'L')) {
        *info = -1;
    } else if (N < 0) {
        *info = -2;
    } else if (NRHS < 0) {
        *info = -3;
    } else if (LDA < MAX(1, N)) {
        *info = -5;
    } else if (LDB < MAX(1, N)) {
        *info = -9;
    }
    int neg_info = -*info;
    if (*info != 0) {
        Xerbla("SSYTRS_3", &neg_info, 10);
    }
    /*
     *     Quick return if possible
     */
    if (N == 0 || NRHS == 0) {
        return;
    }
    if (upper) {
        /*
         *        Begin Upper
         *
         *        Solve A*X = B, where A = U*D*U**T.
         *
         *        P**T * B
         *
         *        Interchange rows K and IPIV(K) of matrix B in the same order
         *        that the formation order of IPIVi vector for Upper case.
         *
         *        (We can do the simple loop over IPIV with decrement -1,
         *        since the ABS value of IPIVi represents the row index
         *        of the interchange with row i in both 1x1 and 2x2 pivot cases)
         */
        //#pragma omp parallel for schedule(static) private(k, kp)
        for (k = N; k > 0; k--) {
            kp = ABS_(ipiv[k - 1]);
            if (kp != k) {
                SWAP_(NRHS, B + k - 1, LDB, B + kp - 1, LDB);
            }
        }

        /*
         *        Compute (U \P**T * B) -> B    [ (U \P**T * B) ]
         */

#if defined(COMPLEX) || defined(COMPLEX16)
        TRSM_(CblasColMajor, CblasLeft, CblasUpper, CblasNoTrans, CblasUnit, N,
              NRHS, (void*)&T_ONE, A, LDA, B, LDB);
#else
        TRSM_(CblasColMajor, CblasLeft, CblasUpper, CblasNoTrans, CblasUnit, N,
              NRHS, T_ONE, A, LDA, B, LDB);
#endif
        /*
         *        Compute D \ B -> B   [ D \ (U \P**T * B) ]
         */
        i = N;
        while (i > 0) {
            if (ipiv[i - 1] > 0) {
#if defined(COMPLEX) || defined(COMPLEX16)
                dataType ALPHA_ = T_ONE / A[i - 1 + (i - 1) * LDA];
                SCAL_(NRHS, (void*)&ALPHA_, B + i - 1, LDB);
#else
                SCAL_(NRHS, T_ONE / A[i - 1 + (i - 1) * LDA], B + i - 1, LDB);
#endif
            } else if (i > 1) {
                akm1k = E[i - 1];
                akm1 = A[i - 2 + (i - 2) * LDA] / akm1k;
                ak = A[i - 1 + (i - 1) * LDA] / akm1k;
                deNom = akm1 * ak - T_ONE;
#pragma omp parallel for schedule(static) private(j)
                for (j = 1; j <= NRHS; j++) {
                    bkm1 = B[i - 2 + (j - 1) * LDB] / akm1k;
                    bk = B[i - 1 + (j - 1) * LDB] / akm1k;
                    B[i - 2 + (j - 1) * LDB] = (ak * bkm1 - bk) / deNom;
                    B[i - 1 + (j - 1) * LDB] = (akm1 * bk - bkm1) / deNom;
                }
                i--;
            }
            i--;
        }
/*
 *        Compute (U**T \ B) -> B   [ U**T \ (D \ (U \P**T * B) ) ]
 */
#if defined(COMPLEX) || defined(COMPLEX16)
        TRSM_(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasUnit, N,
              NRHS, (void*)&T_ONE, A, LDA, B, LDB);
#else
        TRSM_(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasUnit, N,
              NRHS, T_ONE, A, LDA, B, LDB);
#endif
        /*
         *        P * B  [ P * (U**T \ (D \ (U \P**T * B) )) ]
         *
         *        Interchange rows K and IPIV(K) of matrix B in reverse order
         *        from the formation order of IPIVi vector for Upper case.
         *
         *        (We can do the simple loop over IPIV with increment 1,
         *        since the ABS value of IPIVi represents the row index
         *        of the interchange with row i in both 1x1 and 2x2 pivot cases)
         */
        //#pragma omp parallel for schedule(static) private(k, kp)
        for (k = 1; k <= N; k++) {
            kp = ABS_(ipiv[k - 1]);
            if (kp != k) {
                SWAP_(NRHS, B + k - 1, LDB, B + kp - 1, LDB);
            }
        }
    } else {
        /*
         *        Begin Lower
         *
         *        Solve A*X = B, where A = L*D*L**T.
         *
         *        P**T * B
         *        Interchange rows K and IPIV(K) of matrix B in the same order
         *        that the formation order of IPIVi vector for Lower case.
         *
         *        (We can do the simple loop over IPIV with increment 1,
         *        since the ABS value of IPIVi represents the row index
         *        of the interchange with row i in both 1x1 and 2x2 pivot cases)
         */
        //#pragma omp parallel for schedule(static) private(k, kp)
        for (k = 1; k <= N; k++) {
            kp = ABS_(ipiv[k - 1]);
            if (kp != k) {
                SWAP_(NRHS, B + k - 1, LDB, B + kp - 1, LDB);
            }
        }
/*
 *        Compute (L \P**T * B) -> B    [ (L \P**T * B) ]
 */
#if defined(COMPLEX) || defined(COMPLEX16)
        TRSM_(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit, N,
              NRHS, (void*)&T_ONE, A, LDA, B, LDB);
#else
        TRSM_(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit, N,
              NRHS, T_ONE, A, LDA, B, LDB);
#endif
        /*
         *        Compute D \ B -> B   [ D \ (L \P**T * B) ]
         */

        i = 1;
        while (i <= N) {
            if (ipiv[i - 1] > 0) {
#if defined(COMPLEX) || defined(COMPLEX16)
                dataType ALPHA_ = T_ONE / A[i - 1 + (i - 1) * LDA];
                SCAL_(NRHS, (void*)&ALPHA_, B + i - 1, LDB);
#else
                SCAL_(NRHS, T_ONE / A[i - 1 + (i - 1) * LDA], B + i - 1, LDB);
#endif
            } else if (i < N) {
                akm1k = E[i - 1];
                akm1 = A[i - 1 + (i - 1) * LDA] / akm1k;
                ak = A[i + i * LDA] / akm1k;
                deNom = akm1 * ak - T_ONE;
#pragma omp parallel for schedule(static) private(j)
                for (j = 1; j <= NRHS; j++) {
                    bkm1 = B[i - 1 + (j - 1) * LDB] / akm1k;
                    bk = B[i + (j - 1) * LDB] / akm1k;
                    B[i - 1 + (j - 1) * LDB] = (ak * bkm1 - bk) / deNom;
                    B[i + (j - 1) * LDB] = (akm1 * bk - bkm1) / deNom;
                }
                i++;
            }
            i++;
        }
/*
 *        Compute (L**T \ B) -> B   [ L**T \ (D \ (L \P**T * B) ) ]
 */
#if defined(COMPLEX) || defined(COMPLEX16)
        TRSM_(CblasColMajor, CblasLeft, CblasLower, CblasTrans, CblasUnit, N,
              NRHS, (void*)&T_ONE, A, LDA, B, LDB);
#else
        TRSM_(CblasColMajor, CblasLeft, CblasLower, CblasTrans, CblasUnit, N,
              NRHS, T_ONE, A, LDA, B, LDB);
#endif
        /*
         *        P * B  [ P * (L**T \ (D \ (L \P**T * B) )) ]
         *
         *        Interchange rows K and IPIV(K) of matrix B in reverse order
         *        from the formation order of IPIVi vector for Lower case.
         *
         *        (We can do the simple loop over IPIV with decrement -1,
         *        since the ABS value of IPIVi represents the row index
         *        of the interchange with row i in both 1x1 and 2x2 pivot cases)
         */
        //#pragma omp parallel for schedule(static) private(k, kp)
        for (k = N; k >= 1; k--) {
            kp = ABS_(ipiv[k - 1]);
            if (kp != k) {
                SWAP_(NRHS, B + k - 1, LDB, B + kp - 1, LDB);
            }
        }
        /*
         *        END Lower
         */
    }
    return;
    /*
     *     End of SSYTRS_3
     */
}